#include<TELTrack.h>

#include<TCanvas.h>
#include<TH1F.h>
#include<TH2F.h>
#include<TF1.h>

TELTrack::TELTrack(Int_t RN) :
	RunNumber(RN) {

	glob = new Global(1001);

	Path = new Char_t[200];
	sprintf(Path, "ntuple/tele/TELHits%d.root", RunNumber);

	ff = new TFile(Path);

	if (ff->IsZombie()) {

		cout << "There is no " << Path << " file" << endl;
		return;

	}

	ntuple = (TTree*) ff->Get("TELHits");
	NumberOfEvents = ntuple->GetEntries();

	_useBeamConstraint = false;
	_nTelPlanes = glob->TELNumberOfPlanes + 1;
	_eBeam = glob->Energy;

	// Read position in Z (for sorting), skip layers if requested

	_planeSort = new int[_nTelPlanes];
	_planePosition = new double[_nTelPlanes];

	// Book local geometry arrays

	_planeID = new int[_nTelPlanes];
	_planeShiftX = new double[_nTelPlanes];
	_planeShiftY = new double[_nTelPlanes];
	_planeRotZ = new double[_nTelPlanes];
	_planeThickness = new double[_nTelPlanes];
	_planeX0 = new double[_nTelPlanes];
	_planeResolution = new double[_nTelPlanes];
	_isActive = new bool[_nTelPlanes];
	_nActivePlanes = 0;

	//radaition length for silicon 9.36cm = 93600µm
	_planeX0[0] = 93600.0;
	_planeX0[1] = _planeX0[0];
	_planeX0[2] = _planeX0[0];
	_planeX0[3] = _planeX0[0];

	//thicness of telescope planes
	_planeThickness[0] = glob->TELThickness[0];
	_planeResolution[0] = glob->TELSigmaXCogX[0];
	_planePosition[0] = glob->TELOffsetZ[0];

	for (Int_t n = 2; n < _nTelPlanes; n++) {
		_planeThickness[n] = glob->TELThickness[n - 1];
		_planeResolution[n] = glob->TELSigmaXCogX[n - 1];
		_planePosition[n] = glob->TELOffsetZ[n - 1];
	}//n

	//thicnes of MAPS plane
	_planeThickness[1] = glob->DUTThickness;

	//resolution of MAPS plane
	_planeResolution[1] = 0.0;

	_isActive[0] = true;
	_isActive[1] = false;
	_isActive[2] = true;
	_isActive[3] = true;

	_nActivePlanes = glob->TELNumberOfPlanes;

	// Allocate arrays for track fitting
	_planeX = new double[_nTelPlanes];
	_planeEx = new double[_nTelPlanes];
	_planeY = new double[_nTelPlanes];
	_planeEy = new double[_nTelPlanes];

	_planeDist = new double[_nTelPlanes];
	_planeScat = new double[_nTelPlanes];

	_fitX = new double[_nTelPlanes];
	_fitEx = new double[_nTelPlanes];
	_fitY = new double[_nTelPlanes];
	_fitEy = new double[_nTelPlanes];

	arrayDim = _nTelPlanes * _nTelPlanes;

	_fitArray = new double[arrayDim];
	_nominalFitArray = new double[arrayDim];

	_nominalError = new double[_nTelPlanes];

}
void TELTrack::FindTracks(Int_t Event) {

	Float_t DigX[glob->TELNumberOfPlanes][glob->TELMaxHit];
	Float_t DigY[glob->TELNumberOfPlanes][glob->TELMaxHit];
	Float_t CogX[glob->TELNumberOfPlanes][glob->TELMaxHit];
	Float_t CogY[glob->TELNumberOfPlanes][glob->TELMaxHit];

	ntuple->SetBranchAddress("DigX", &DigX[0][0]);
	ntuple->SetBranchAddress("DigY", &DigY[0][0]);
	ntuple->SetBranchAddress("CogX", &CogX[0][0]);
	ntuple->SetBranchAddress("CogY", &CogY[0][0]);

	if (Event >= NumberOfEvents) {
		cout << " Error - you have superpassed number of events in run " << endl;
		return;
	}

	ntuple->GetEntry(Event);

	SlopeDigX.clear();
	ConstDigX.clear();
	Chi2DigX.clear();
	SlopeDigY.clear();
	ConstDigY.clear();
	Chi2DigY.clear();

	SlopeCogX.clear();
	ConstCogX.clear();
	Chi2CogX.clear();
	SlopeCogY.clear();
	ConstCogY.clear();
	Chi2CogY.clear();

	Int_t while1;
	Bool_t FitInfo;

	Float_t* PlanePosition;
	Float_t* HitPosition;
	Float_t* HitPositionErr;
	Int_t size;

	TF1* f1 = new TF1("f1", "[0]*x + [1]");
	TGraphErrors* LineGraph;

	//DigX
	while1 = 0;
	while (DigX[0][while1] > 0.0 && while1 < glob->TELMaxHit) {
		size = 1;
		FitInfo = false;
		for (Int_t n = 1; n < glob->TELNumberOfPlanes; n++) {
			if (DigX[n][while1] > 0.0) {
				FitInfo = true;
				size++;
			}
		}//n
		PlanePosition = new Float_t[size];
		HitPosition = new Float_t[size];
		HitPositionErr = new Float_t[size];
		if (FitInfo) {
			size = 0;
			for (Int_t n = 0; n < glob->TELNumberOfPlanes; n++) {
				if (DigX[n][while1] > 0.0) {
					PlanePosition[size] = glob->TELOffsetZ[n];
					HitPosition[size] = DigX[n][while1];
					HitPositionErr[size] = glob->TELStripSizeX[0] / TMath::Sqrt(12.0);
					size++;
				}
			}//n
			LineGraph = new TGraphErrors(size, PlanePosition, HitPosition, 0, HitPositionErr);
			LineGraph->Fit("f1", "Q");

			SlopeDigX.push_back(f1->GetParameter(0));
			ConstDigX.push_back(f1->GetParameter(1));
			Chi2DigX.push_back(f1->GetChisquare());

			delete LineGraph;
		}
		delete[] PlanePosition;
		delete[] HitPosition;
		delete[] HitPositionErr;
		while1++;
	}//while1
	//DigY
	while1 = 0;
	while (DigY[0][while1] > 0.0 && while1 < glob->TELMaxHit) {
		size = 1;
		FitInfo = false;
		for (Int_t n = 1; n < glob->TELNumberOfPlanes; n++) {
			if (DigY[n][while1] > 0.0) {
				FitInfo = true;
				size++;
			}
		}//n
		PlanePosition = new Float_t[size];
		HitPosition = new Float_t[size];
		HitPositionErr = new Float_t[size];
		if (FitInfo) {
			size = 0;
			for (Int_t n = 0; n < glob->TELNumberOfPlanes; n++) {
				if (DigY[n][while1] > 0.0) {
					PlanePosition[size] = glob->TELOffsetZ[n];
					HitPosition[size] = DigY[n][while1];
					HitPositionErr[size] = glob->TELStripSizeY[0] / TMath::Sqrt(12.0);
					size++;
				}
			}//n
			LineGraph = new TGraphErrors(size, PlanePosition, HitPosition, 0, HitPositionErr);
			LineGraph->Fit("f1", "Q");

			SlopeDigY.push_back(f1->GetParameter(0));
			ConstDigY.push_back(f1->GetParameter(1));
			Chi2DigY.push_back(f1->GetChisquare());

			delete LineGraph;
		}
		delete[] PlanePosition;
		delete[] HitPosition;
		delete[] HitPositionErr;
		while1++;
	}//while1
	//CogX
	while1 = 0;
	while (CogX[0][while1] > 0.0 && while1 < glob->TELMaxHit) {
		size = 1;
		FitInfo = false;
		for (Int_t n = 1; n < glob->TELNumberOfPlanes; n++) {
			if (CogX[n][while1] > 0.0) {
				FitInfo = true;
				size++;
			}
		}//n
		PlanePosition = new Float_t[size];
		HitPosition = new Float_t[size];
		HitPositionErr = new Float_t[size];
		if (FitInfo) {
			size = 0;
			for (Int_t n = 0; n < glob->TELNumberOfPlanes; n++) {
				if (CogX[n][while1] > 0.0) {
					PlanePosition[size] = glob->TELOffsetZ[n];
					HitPosition[size] = CogX[n][while1];
					HitPositionErr[size] = glob->TELSigmaXCogX[n];
					size++;
				}
			}//n
			LineGraph = new TGraphErrors(size, PlanePosition, HitPosition, 0, HitPositionErr);
			LineGraph->Fit("f1", "Q");

			SlopeCogX.push_back(f1->GetParameter(0));
			ConstCogX.push_back(f1->GetParameter(1));
			Chi2CogX.push_back(f1->GetChisquare());

			delete LineGraph;
		}
		delete[] PlanePosition;
		delete[] HitPosition;
		delete[] HitPositionErr;
		while1++;
	}//while1

	//CogY
	while1 = 0;
	while (CogY[0][while1] > 0.0 && while1 < glob->TELMaxHit) {
		size = 1;
		FitInfo = false;
		for (Int_t n = 1; n < glob->TELNumberOfPlanes; n++) {
			if (CogY[n][while1] > 0.0) {
				FitInfo = true;
				size++;
			}
		}//n
		PlanePosition = new Float_t[size];
		HitPosition = new Float_t[size];
		HitPositionErr = new Float_t[size];
		if (FitInfo) {
			size = 0;
			for (Int_t n = 0; n < glob->TELNumberOfPlanes; n++) {
				if (CogY[n][while1] > 0.0) {
					PlanePosition[size] = glob->TELOffsetZ[n];
					HitPosition[size] = CogY[n][while1];
					HitPositionErr[size] = glob->TELSigmaXCogY[n];
					size++;
				}
			}//n
			LineGraph = new TGraphErrors(size, PlanePosition, HitPosition, 0, HitPositionErr);
			LineGraph->Fit("f1", "Q");

			SlopeCogY.push_back(f1->GetParameter(0));
			ConstCogY.push_back(f1->GetParameter(1));
			Chi2CogY.push_back(f1->GetChisquare());

			delete LineGraph;
		}
		delete[] PlanePosition;
		delete[] HitPosition;
		delete[] HitPositionErr;
		while1++;
	}//while1
}//FindTrack
void TELTrack::FindHits(Int_t Event) {

	Float_t DigX[glob->TELNumberOfPlanes][glob->TELMaxHit];
	Float_t DigY[glob->TELNumberOfPlanes][glob->TELMaxHit];
	Float_t CogX[glob->TELNumberOfPlanes][glob->TELMaxHit];
	Float_t CogY[glob->TELNumberOfPlanes][glob->TELMaxHit];

	ntuple->SetBranchAddress("DigX", &DigX[0][0]);
	ntuple->SetBranchAddress("DigY", &DigY[0][0]);
	ntuple->SetBranchAddress("CogX", &CogX[0][0]);
	ntuple->SetBranchAddress("CogY", &CogY[0][0]);

	if (Event >= NumberOfEvents) {

		cout << " Error - you have superpassed number of events in run " << endl;
		return;

	}

	ntuple->GetEntry(Event);

	Float_t PosX[glob->TELNumberOfPlanes];
	Float_t PosY[glob->TELNumberOfPlanes];

	HitDigX.clear();
	HitDigY.clear();
	Chi2Dig.clear();

	HitCogX.clear();
	HitCogY.clear();
	Chi2Cog.clear();

	Int_t while1;
	Bool_t FitInfo;

	//Dig
	//resolution of telescope planes
	_planeResolution[0] = glob->TELStripSizeX[0] / TMath::Sqrt(12.0);

	for (Int_t n = 2; n < _nTelPlanes; n++) {
		_planeResolution[n] = glob->TELStripSizeX[n - 1] / TMath::Sqrt(12.0);
	}//n

	AFZMatrix();

	while1 = 0;
	while (DigX[0][while1] > 0.0 && DigY[0][while1] > 0.0 && while1 < glob->TELMaxHit) {
		FitInfo = true;
		PosX[0] = DigX[0][while1];
		PosY[0] = DigY[0][while1];
		for (Int_t n = 1; n < glob->TELNumberOfPlanes; n++) {
			if (DigX[n][while1] < 0.0 || DigY[n][while1] < 0.0) {
				FitInfo = false;
				continue;
			}
			PosX[n] = DigX[n][while1];
			PosY[n] = DigY[n][while1];
		}//n
		if (FitInfo) {
			Chi2Dig.push_back(AFZFit(PosX, PosY));
			HitDigX.push_back(_fitX[1]);
			HitDigY.push_back(_fitY[1]);
		}//if
		while1++;
	}//while1

	//Cog
	//resolution of telescope planes
	_planeResolution[0] = glob->TELSigmaXCogX[0];

	for (Int_t n = 2; n < _nTelPlanes; n++) {
		_planeResolution[n] = glob->TELSigmaXCogX[n - 1];
	}//n

	AFZMatrix();

	while1 = 0;
	while (CogX[0][while1] > 0.0 && CogY[0][while1] > 0.0 && while1 < glob->TELMaxHit) {
		FitInfo = true;
		PosX[0] = CogX[0][while1];
		PosY[0] = CogY[0][while1];
		for (Int_t n = 1; n < glob->TELNumberOfPlanes; n++) {
			if (DigX[n][while1] < 0.0 || DigY[n][while1] < 0.0) {
				FitInfo = false;
				continue;
			}
			PosX[n] = CogX[n][while1];
			PosY[n] = CogY[n][while1];
		}//n
		if (FitInfo) {
			Chi2Cog.push_back(AFZFit(PosX, PosY));
			HitCogX.push_back(_fitX[1]);
			HitCogY.push_back(_fitY[1]);
		}//if
		while1++;
	}//while1
}//FindHits
void TELTrack::GetHits(Int_t Event) {

	Float_t CogX[glob->TELNumberOfPlanes][glob->TELMaxHit];
	Float_t CogY[glob->TELNumberOfPlanes][glob->TELMaxHit];

	ntuple->SetBranchAddress("CogX", &CogX[0][0]);
	ntuple->SetBranchAddress("CogY", &CogY[0][0]);

	if (Event >= NumberOfEvents) {

		cout << " Error - you have superpassed number of events in run " << endl;
		return;

	}

	ntuple->GetEntry(Event);

	Float_t PosX[glob->TELNumberOfPlanes];
	Float_t PosY[glob->TELNumberOfPlanes];

	HitCogX.clear();
	HitCogY.clear();
	Chi2Cog.clear();

	Int_t while1;
	Bool_t FitInfo;

	//Cog
	//resolution of telescope planes
	_planeResolution[0] = glob->TELSigmaXCogX[0];

	for (Int_t n = 2; n < _nTelPlanes; n++) {
		_planeResolution[n] = glob->TELSigmaXCogX[n - 1];
	}//n

	AFZMatrix();

	while1 = 0;
	while (CogX[0][while1] > 0.0 && CogY[0][while1] > 0.0 && while1 < glob->TELMaxHit) {

		FitInfo = true;

		PosX[0] = CogX[0][while1];
		PosY[0] = CogY[0][while1];

		for (Int_t n = 1; n < glob->TELNumberOfPlanes; n++) {

			if (CogX[n][while1] < 0.0 || CogY[n][while1] < 0.0) {
				FitInfo = false;
				continue;
			}

			PosX[n] = CogX[n][while1];
			PosY[n] = CogY[n][while1];

		}//n

		if (FitInfo) {

			Chi2Cog.push_back(AFZFit(PosX, PosY));
			HitCogX.push_back(_fitX[1]);
			HitCogY.push_back(_fitY[1]);

		}//if

		while1++;

	}//while1

}//GetHits
Int_t TELTrack::DoAnalFit(Double_t * pos, Double_t *err) {
	for (int ipl = 0; ipl < _nTelPlanes; ipl++) {
		if (_isActive[ipl] && err[ipl] > 0)
			err[ipl] = 1. / err[ipl] / err[ipl];
		else
			err[ipl] = 0.;
		pos[ipl] *= err[ipl];
	}

	for (int ipl = 0; ipl < _nTelPlanes; ipl++)
		for (int jpl = 0; jpl < _nTelPlanes; jpl++) {
			int imx = ipl + jpl * _nTelPlanes;
			_fitArray[imx] = 0.;
			if (jpl == ipl - 2)
				_fitArray[imx] += _planeDist[ipl - 2] * _planeDist[ipl - 1] * _planeScat[ipl - 1];
			if (jpl == ipl + 2)
				_fitArray[imx] += _planeDist[ipl] * _planeDist[ipl + 1] * _planeScat[ipl + 1];
			if (jpl == ipl - 1) {
				if (ipl > 0 && ipl < _nTelPlanes - 1)
					_fitArray[imx] -= _planeDist[ipl - 1] * (_planeDist[ipl] + _planeDist[ipl - 1]) * _planeScat[ipl];
				if (ipl > 1)
					_fitArray[imx] -= _planeDist[ipl - 1] * (_planeDist[ipl - 1] + _planeDist[ipl - 2]) * _planeScat[ipl - 1];
			}
			if (jpl == ipl + 1) {
				if (ipl > 0 && ipl < _nTelPlanes - 1)
					_fitArray[imx] -= _planeDist[ipl] * (_planeDist[ipl] + _planeDist[ipl - 1]) * _planeScat[ipl];
				if (ipl < _nTelPlanes - 2)
					_fitArray[imx] -= _planeDist[ipl] * (_planeDist[ipl + 1] + _planeDist[ipl]) * _planeScat[ipl + 1];
			}

			if (jpl == ipl) {
				_fitArray[imx] += err[ipl];
				if (ipl > 0 && ipl < _nTelPlanes - 1)
					_fitArray[imx] += _planeScat[ipl] * (_planeDist[ipl] + _planeDist[ipl - 1]) * (_planeDist[ipl] + _planeDist[ipl - 1]);
				if (ipl > 1)
					_fitArray[imx] += _planeScat[ipl - 1] * _planeDist[ipl - 1] * _planeDist[ipl - 1];
				if (ipl < _nTelPlanes - 2)
					_fitArray[imx] += _planeScat[ipl + 1] * _planeDist[ipl] * _planeDist[ipl];
			}
			// For beam constraint
			if (ipl == jpl && ipl < 2 && _useBeamConstraint)
				_fitArray[imx] += _planeScat[0] * _planeDist[1] * _planeDist[1];
			if (ipl + jpl == 1 && _useBeamConstraint)
				_fitArray[imx] -= _planeScat[0] * _planeDist[1] * _planeDist[1];
		}

	int status = GaussjSolve(_fitArray, pos, _nTelPlanes);

	if (status) {
		cerr << "Singular matrix in track fitting algorithm ! " << endl;
		for (int ipl = 0; ipl < _nTelPlanes; ipl++)
			err[ipl] = 0.;
	} else
		for (int ipl = 0; ipl < _nTelPlanes; ipl++)
			err[ipl] = sqrt(_fitArray[ipl + ipl * _nTelPlanes]);

	return status;

}//DoAnalFit
Double_t TELTrack::GetFitChi2() {
	double chi2 = 0.;

	// Measurements

	for (int ipl = 0; ipl < _nTelPlanes; ipl++)
		if (_isActive[ipl]) {
			if (_planeEx[ipl] > 0.)
				chi2 += (_fitX[ipl] - _planeX[ipl]) * (_fitX[ipl] - _planeX[ipl]) / _planeEx[ipl] / _planeEx[ipl];

			if (_planeEy[ipl] > 0.)
				chi2 += (_fitY[ipl] - _planeY[ipl]) * (_fitY[ipl] - _planeY[ipl]) / _planeEy[ipl] / _planeEy[ipl];

		}

	// Scattering angles
	// Use approximate formulas, corresponding to the approximation
	// used in fitting algorithm

	for (int ipl = 1; ipl < _nTelPlanes - 1; ipl++) {
		double th1, th2, dth;

		th2 = (_fitX[ipl + 1] - _fitX[ipl]) * _planeDist[ipl];
		th1 = (_fitX[ipl] - _fitX[ipl - 1]) * _planeDist[ipl - 1];
		//    dth=atan(th2)-atan(th1) ;
		dth = th2 - th1;
		chi2 += _planeScat[ipl] * dth * dth;

		th2 = (_fitY[ipl + 1] - _fitY[ipl]) * _planeDist[ipl];
		th1 = (_fitY[ipl] - _fitY[ipl - 1]) * _planeDist[ipl - 1];
		//    dth=atan(th2)-atan(th1) ;
		dth = th2 - th1;
		chi2 += _planeScat[ipl] * dth * dth;
	}

	// Beam constraint

	if (_useBeamConstraint) {
		double dth;

		//    dth=atan((_fitX[1]-_fitX[0])*_planeDist[0]) ;
		dth = (_fitX[1] - _fitX[0]) * _planeDist[0];
		chi2 += _planeScat[0] * dth * dth;

		//    dth=atan((_fitY[1]-_fitY[0])*_planeDist[0]) ;
		dth = (_fitY[1] - _fitY[0]) * _planeDist[0];
		chi2 += _planeScat[0] * dth * dth;
	}

	return chi2;
}//GetFitChi2
Int_t TELTrack::GaussjSolve(Double_t *alfa, Double_t *beta, int n) {
	int *ipiv;
	int *indxr;
	int *indxc;
	int i, j, k;
	int irow = 0;
	int icol = 0;
	double abs, big, help, pivinv;

	ipiv = new int[n];
	indxr = new int[n];
	indxc = new int[n];

	for (i = 0; i < n; ipiv[i++] = 0)
		;

	for (i = 0; i < n; i++) {
		big = 0.;
		for (j = 0; j < n; j++) {
			if (ipiv[j] == 1)
				continue;
			for (k = 0; k < n; k++) {
				if (ipiv[k] != 0)
					continue;
				abs = fabs(alfa[n * j + k]);
				if (abs > big) {
					big = abs;
					irow = j;
					icol = k;
				}
			}
		}
		ipiv[icol]++;

		if (ipiv[icol] > 1)
			return 1;

		if (irow != icol) {
			help = beta[irow];
			beta[irow] = beta[icol];
			beta[icol] = help;
			for (j = 0; j < n; j++) {
				help = alfa[n * irow + j];
				alfa[n * irow + j] = alfa[n * icol + j];
				alfa[n * icol + j] = help;
			}
		}
		indxr[i] = irow;
		indxc[i] = icol;

		if (alfa[n * icol + icol] == 0.)
			return 1;

		help = alfa[n * icol + icol];
		pivinv = 1. / help;
		alfa[n * icol + icol] = 1.;
		for (j = 0; j < n; alfa[n * icol + (j++)] *= pivinv)
			;
		beta[icol] *= pivinv;

		for (j = 0; j < n; j++) {
			if (j == icol)
				continue;
			help = alfa[n * j + icol];
			alfa[n * j + icol] = 0.;
			for (k = 0; k < n; k++)
				alfa[n * j + k] -= alfa[n * icol + k] * help;
			beta[j] -= beta[icol] * help;
		}
	}

	for (i = n - 1; i >= 0; i--) {
		if (indxr[i] == indxc[i])
			continue;
		for (j = 0; j < n; j++) {
			help = alfa[n * j + indxr[i]];
			alfa[n * j + indxr[i]] = alfa[n * j + indxc[i]];
			alfa[n * j + indxc[i]] = help;
		}
	}

	delete[] ipiv;
	delete[] indxr;
	delete[] indxc;

	return 0;
}//GaussjSolve
void TELTrack::AFZMatrix() {

	Float_t MAPSTheta = TMath::ACos(TMath::Cos(glob->DUTPhiX) * TMath::Cos(glob->DUTPhiY));
	_planeThickness[1] = glob->DUTThickness / TMath::Cos(MAPSTheta);

	// Fill nominal fit matrices and
	// calculate expected precision of track fitting

	// Planes have to be ordered in position along the beam line !
	// This is not checked !!!

	_planePosition[1] = glob->DUTOffsetZ;

	for (int ipl = 0; ipl < _nTelPlanes; ipl++) {
		if (ipl > 0)
			_planeDist[ipl - 1] = 1. / (_planePosition[ipl] - _planePosition[ipl - 1]);

		_planeScat[ipl] = 0.0136 / _eBeam * sqrt(_planeThickness[ipl] / _planeX0[ipl]) * (1. + 0.038 * TMath::Log(_planeThickness[ipl] / _planeX0[ipl]));

		if (ipl == 0 && _useBeamConstraint)
			_planeScat[ipl] = 1. / (_planeScat[ipl] * _planeScat[ipl] + _beamSpread * _beamSpread);
		else
			_planeScat[ipl] = 1. / (_planeScat[ipl] * _planeScat[ipl]);

		_fitX[ipl] = _fitY[ipl] = 0.;
		_nominalError[ipl] = _planeResolution[ipl];
	}

	// Fit with nominal parameters
	int status = DoAnalFit(_fitX, _nominalError);

	if (status)
		cerr << "\n Fit with nominal geometry failed !?!" << endl;

	// Store fit matrix

	for (int imx = 0; imx < arrayDim; imx++)
		_nominalFitArray[imx] = _fitArray[imx];

}
Double_t TELTrack::NominalFit() {
	for (int ipl = 0; ipl < _nTelPlanes; ipl++) {
		_fitEy[ipl] = _fitEx[ipl] = _nominalError[ipl];

		_fitX[ipl] = 0.;
		_fitY[ipl] = 0.;
		for (int jpl = 0; jpl < _nTelPlanes; jpl++) {
			if (_planeEx[jpl] > 0.)
				_fitX[ipl] += _nominalFitArray[ipl + jpl * _nTelPlanes] * _planeX[jpl] / _planeEx[jpl] / _planeEx[jpl];
			if (_planeEy[jpl] > 0.)
				_fitY[ipl] += _nominalFitArray[ipl + jpl * _nTelPlanes] * _planeY[jpl] / _planeEy[jpl] / _planeEy[jpl];
		}
	}

	Double_t chi2 = GetFitChi2();

	return chi2;
}
Double_t TELTrack::AFZFit(Float_t PosX[], Float_t PosY[]) {

	Double_t Chi2;

	_planeX[0] = PosX[0];
	_planeX[1] = 0.0;
	_planeEx[0] = _planeResolution[0];
	_planeEx[1] = 0.0;

	_planeY[0] = PosY[0];
	_planeY[1] = 0.0;
	_planeEy[0] = _planeResolution[0];
	_planeEy[1] = 0.0;

	for (Int_t n = 2; n < _nTelPlanes; n++) {

		_planeX[n] = PosX[n - 1];
		_planeEx[n] = _planeResolution[n];

		_planeY[n] = PosY[n - 1];
		_planeEy[n] = _planeResolution[n];

	}//n

	_planeX[2] = PosX[1];
	_planeX[3] = PosX[2];
	_planeEx[2] = _planeResolution[2];
	_planeEx[3] = _planeResolution[3];

	_planeY[2] = PosY[1];
	_planeY[3] = PosY[2];
	_planeEy[2] = _planeResolution[2];
	_planeEy[3] = _planeResolution[3];

	Chi2 = NominalFit();

	/*
	 cout << "Chi2 " << Chi2 << endl;

	 cout << "X" << endl;
	 cout << PosX[0] << " " << _fitX[0] << endl;
	 cout << PosX[1] << " " << _fitX[2] << endl;
	 cout << PosX[2] << " " << _fitX[3] << endl;
	 cout << "MAPS X" << endl;
	 cout << _fitX[1] << endl << endl;

	 cout << "Y" << endl;
	 cout << PosY[0] << " " << _fitY[0] << endl;
	 cout << PosX[1] << " " << _fitY[2] << endl;
	 cout << PosX[2] << " " << _fitY[3] << endl;
	 cout << "MAPS Y" << endl;
	 cout << _fitY[1] << endl << endl;
	 */

	return Chi2;

}
void TELTrack::PlotTracks() {

	//Track Parameters Dig
	TCanvas* TELTrackcanv11 = new TCanvas("TELTrackc1", "", 1200, 800);
	TELTrackcanv11->SetFillColor(0);
	TELTrackcanv11->Divide(3, 2);

	TH1F* x_a_Dig = new TH1F("slopexDig", "x slope", 200, -0.05, 0.05);
	TH1F* x_b_Dig = new TH1F("bxDig", "b constant for x track", 100, 0, glob->TELNumberOfStripsX[0] * glob->TELStripSizeX[0]);
	TH1F* y_a_Dig = new TH1F("slopeyDig", "y slope", 200, -0.05, 0.05);
	TH1F* y_b_Dig = new TH1F("byDig", "b constant for y track", 100, 0, glob->TELNumberOfStripsY[0] * glob->TELStripSizeY[0]);
	TH1F* x_chi2_Dig = new TH1F("x_chi2Dig", "#chi^{2} x", 100, -1, 100);
	TH1F* y_chi2_Dig = new TH1F("y_chi2Dig", "#chi^{2} y", 100, -1, 100);

	//Track Parameters Cog
	TCanvas* TELTrackcanv12 = new TCanvas("TELTrackc2", "", 1200, 800);
	TELTrackcanv12->SetFillColor(0);
	TELTrackcanv12->Divide(3, 2);

	TH1F* x_a_Cog = new TH1F("slopexCog", "x slope", 200, -0.05, 0.05);
	TH1F* x_b_Cog = new TH1F("bxCog", "b constant for x track", 100, 0, glob->TELNumberOfStripsX[0] * glob->TELStripSizeX[0]);
	TH1F* y_a_Cog = new TH1F("slopeyCog", "y slope", 200, -0.05, 0.05);
	TH1F* y_b_Cog = new TH1F("byCog", "b constant for y track", 100, 0, glob->TELNumberOfStripsY[0] * glob->TELStripSizeY[0]);
	TH1F* x_chi2_Cog = new TH1F("x_chi2Cog", "#chi^{2} x", 100, -1, 100);
	TH1F* y_chi2_Cog = new TH1F("y_chi2Cog", "#chi^{2} y", 100, -1, 100);

	TCanvas* TELTrackcanv13 = new TCanvas("TELTrackc3", "", 1200, 600);
	TELTrackcanv13->SetFillColor(0);
	TELTrackcanv13->Divide(2);

	TCanvas* TELTrackcanv14 = new TCanvas("TELTrackc4", "", 600, 600);
	TELTrackcanv14->SetFillColor(0);

	TH1F* Chi2AFZDig = new TH1F("Chi2AFZDig", "#chi^{2} Dig", 200, 0, 100);
	TH1F* Chi2AFZCog = new TH1F("Chi2AFZCog", "#chi^{2} Cog", 200, 0, 100);

	//TF1* Chi2Fun1 = new TF1("Chi2Fun1","[0]*x^([1]/2.0 - 1.0)*exp(-x/2.0)",0,20);
	//TF1* Chi2Fun2 = new TF1("Chi2Fun2","[0]*x^([1]/2.0 - 1.0)*exp(-x/2.0)",0,20);
	TF1* Chi2Fun1 = new TF1("Chi2Fun1", "[0]*exp(-[1]*x)", 0, 30);
	TF1* Chi2Fun2 = new TF1("Chi2Fun2", "[0]*exp(-[1]*x)", 0, 30);

	for (Int_t i = 0; i < NumberOfEvents; i++) {
		FindTracks(i);
		for (Int_t n = 0; n < SlopeDigX.size(); n++) {
			x_a_Dig->Fill(SlopeDigX[n]);
			x_b_Dig->Fill(ConstDigX[n]);
			x_chi2_Dig->Fill(Chi2DigX[n]);
		}
		for (Int_t n = 0; n < SlopeDigY.size(); n++) {
			y_a_Dig->Fill(SlopeDigY[n]);
			y_b_Dig->Fill(ConstDigY[n]);
			y_chi2_Dig->Fill(Chi2DigY[n]);
		}
		for (Int_t n = 0; n < SlopeCogX.size(); n++) {
			x_a_Cog->Fill(SlopeCogX[n]);
			x_b_Cog->Fill(ConstCogX[n]);
			x_chi2_Cog->Fill(Chi2CogX[n]);
		}
		for (Int_t n = 0; n < SlopeCogY.size(); n++) {
			y_a_Cog->Fill(SlopeCogY[n]);
			y_b_Cog->Fill(ConstCogY[n]);
			y_chi2_Cog->Fill(Chi2CogY[n]);
		}
		FindHits(i);
		for (Int_t n = 0; n < Chi2Dig.size(); n++)
			Chi2AFZDig->Fill(Chi2Dig[n]);
		for (Int_t n = 0; n < Chi2Cog.size(); n++)
			Chi2AFZCog->Fill(Chi2Cog[n]);
	}//i
	//Cog
	//resolution of telescope planes
	_planeResolution[0] = glob->TELSigmaXCogX[0];

	for (Int_t n = 2; n < _nTelPlanes; n++) {
		_planeResolution[n] = glob->TELSigmaXCogX[n - 1];
	}//n

	AFZMatrix();

	cout << "**********************************************" << endl;
	cout << "Error of the position reconstruction in:" << endl;
	for (int ipl = 0; ipl < _nTelPlanes; ipl++) {
		cout << "plane nr " << ipl << " - " << _nominalError[ipl] << endl;
	}
	cout << "**********************************************" << endl;

	TELTrackcanv11->cd(1);
	x_a_Dig->Draw();
	TELTrackcanv11->cd(2);
	x_b_Dig->Draw();
	TELTrackcanv11->cd(3);
	x_chi2_Dig->Draw();

	TELTrackcanv11->cd(4);
	y_a_Dig->Draw();
	TELTrackcanv11->cd(5);
	y_b_Dig->Draw();
	TELTrackcanv11->cd(6);
	y_chi2_Dig->Draw();

	TELTrackcanv12->cd(1);
	x_a_Cog->Draw();
	TELTrackcanv12->cd(2);
	x_b_Cog->Draw();
	TELTrackcanv12->cd(3);
	x_chi2_Cog->Draw();

	TELTrackcanv12->cd(4);
	y_a_Cog->Draw();
	TELTrackcanv12->cd(5);
	y_b_Cog->Draw();
	TELTrackcanv12->cd(6);
	y_chi2_Cog->Draw();

	TELTrackcanv13->cd(1);
	Chi2AFZDig->Draw();
	Chi2Fun2->SetLineColor(3);
	Chi2AFZDig->Fit("Chi2Fun2");
	TELTrackcanv13->cd(2);
	Chi2AFZCog->Draw();
	Chi2Fun1->SetLineColor(5);
	Chi2AFZCog->Fit("Chi2Fun1");

	TELTrackcanv14->cd();
	Chi2AFZDig->SetLineColor(4);
	Chi2AFZDig->SetLineWidth(2);
	Chi2AFZDig->Draw();
	Chi2AFZCog->SetLineColor(2);
	Chi2AFZCog->SetLineWidth(2);
	Chi2AFZCog->Draw("SAME");

}//PlotTracks
TELTrack::~TELTrack() {

	// Clean memory

	delete[] _planeSort;
	delete[] _planeID;
	delete[] _planeShiftX;
	delete[] _planeShiftY;
	delete[] _planeRotZ;
	delete[] _planePosition;
	delete[] _planeThickness;
	delete[] _planeX0;
	delete[] _planeResolution;
	delete[] _planeDist;
	delete[] _planeScat;
	delete[] _isActive;

	delete[] _planeX;
	delete[] _planeEx;
	delete[] _planeY;
	delete[] _planeEy;

	delete[] _fitX;
	delete[] _fitEx;
	delete[] _fitY;
	delete[] _fitEy;
	delete[] _fitArray;

	delete[] _nominalFitArray;
	delete[] _nominalError;

	delete[] Path;
	//ff->Close();

}
